#include "ros/ros.h"
#include <std_msgs/Int32.h>
#include "roboard_drivers/Encoder.h"
#include "roboard_drivers/Motor.h"
#include <string.h>
#include <signal.h>
#include <termios.h>
#include <stdio.h>

#define KEYCODE_R 0x43 
#define KEYCODE_L 0x44
#define KEYCODE_U 0x41
#define KEYCODE_D 0x42
#define KEYCODE_Q 0x71
#define KEYCODE_S 0x31

using namespace roboard_drivers;
ros::Publisher direction_pub;


int kfd = 0;
struct termios cooked, raw;

void quit(int sig)
{
  tcsetattr(kfd, TCSANOW, &cooked);
  ros::shutdown();
  exit(0);
}

int main(int argc, char** argv)
{
  std_msgs::Int32 dir;
  ros::init(argc, argv, "TeleopControlNode");
  signal(SIGINT,quit);
  ros::NodeHandle n;
  direction_pub = n.advertise<std_msgs::Int32>("/serial/direction", 1000);
  
  char c;

  // get the console in raw mode                                                              
  tcgetattr(kfd, &cooked);
  memcpy(&raw, &cooked, sizeof(struct termios));
  raw.c_lflag &=~ (ICANON | ECHO);
  // Setting a new line, then end of file                         
  raw.c_cc[VEOL] = 1;
  raw.c_cc[VEOF] = 2;
  tcsetattr(kfd, TCSANOW, &raw);

  puts("Use arrow keys to control the robot.");
  puts("Reading from keyboard:");

  while (ros::ok())
  {
	// get the next event from the keyboard  
    if(read(kfd, &c, 1) < 0)
    {
      perror("read():");
      exit(-1);
    }

  dir.data = 0;

    switch(c)
    {
      case KEYCODE_L:
	puts("LEFT ");
        dir.data = 4;
        break;
      case KEYCODE_R:
	puts("RIGHT");
        dir.data = 3;
        break;
      case KEYCODE_U:
	puts("UP");
        dir.data = 1;
        break;
      case KEYCODE_D:
	puts("DOWN");
        dir.data = 2;
        break;
      case KEYCODE_S:
	puts("STOP");
        dir.data = 5;
        break;

    }
	if(dir.data!=0)
	    	direction_pub.publish(dir);

    ros::spinOnce();
  }
  
  return(0);
}



